clc
clear
close all

m=0.2;
% i_xx = 0;
% i_yy = 0;

A = [0 0 1 0; 0 0 0 1;0 0 0 0;0 0 0 0];
B = [0 0;0 0;1/m 0;0 1/m];
C = eye(4);
D =0;
Ts = 0.001;
t = 0:Ts:15;
u = zeros(length(t), 2); % 修改这里，确保u有两列
[G , H] = c2d(A,B,Ts);
x0 = [1;1;0;0];
Tc = ctrb(G,H);
if (rank(Tc)==4)
    Q = [10 0 0 0; 
        0 10 0 0;
        0 0 10 0;
        0 0 0 10];
    R = 10;
    K1 = dlqr(G,H,Q,R);
    G2 = G-H*K1;
    y = dlsim(G2,H,C,D,u,x0);

    subplot(4,1,1);
    plot(t,y(:,1),'b','LineWidth',1.5);
    xlabel('x_pos');
    ylabel('m');

    subplot(4,1,2);
    plot(t,y(:,2),'b','LineWidth',1.5);
    xlabel('y_pos');
    ylabel('m');

    subplot(4,1,3);
    plot(t,y(:,3),'b','LineWidth',1.5);
    xlabel('x_vel');
    ylabel('m');

    subplot(4,1,4);
    plot(t,y(:,4),'b','LineWidth',1.5);
    xlabel('y_vel');
    ylabel('m');

    grid on;
end

disp(K1)
